/*
 * forage.java
 */

import java.lang.Thread.State;
import java.lang.reflect.Array;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.Set;
import java.util.concurrent.locks.ReentrantLock;

import javax.rmi.CORBA.Util;

import EDU.gatech.cc.is.util.Units;
import	EDU.gatech.cc.is.util.Vec2;
import	EDU.gatech.cc.is.abstractrobot.*;
import	EDU.gatech.cc.is.clay.*;

public class test  extends ControlSystemMFN150
{
	static ReentrantLock synchron = new ReentrantLock();
	public final static boolean DEBUG = true;

	MultiForageN150 bot;
	long timestamp = 0;
	
	void print(String s){
		synchron.lock();
		abstract_robot.setDisplayString(s);
		System.err.println("BOT "+abstract_robot.getID()+" STATE: "+s);
		synchron.unlock();
	}
		

	void bot_turn(long t, double ang){
		bot.setSteerHeading(t, bot.getSteerHeading(t)+ang);
		bot.setTurretHeading(t, bot.getTurretHeading(t)+ang);
	}
	void bot_turn(double ang){
		bot_turn(timestamp, ang);
	}
	
	Vec2 getClosest(Vec2[] vec, Vec2 p){
		if(vec.length==0) return null;
		Vec2 c = vec[0];
		for(Vec2 v: vec){
			Vec2 vv = new Vec2(v); //vv.sub(p);
			Vec2 cc = new Vec2(c); //cc.sub(p);
			if(vv.r<cc.r) c = v;
		}
		Vec2 res = new Vec2(c); res.add(p);
		return res;
	}
	
	Vec2D myPos(){ return new Vec2D(bot.getPosition(timestamp)); }
	Vec2D closestObstacle(){ 
		Vec2 v = getClosest(bot.getObstacles(timestamp), bot.getPosition(timestamp));
		//Vec2 v = getClosest(bot.getVisualObjects(timestamp, 0xC0C0C0), bot.getPosition(timestamp));
		if(v==null) return null;
		return new Vec2D(v);
	}
	
	
	Vec2 copy(Vec2 v){ return new Vec2(v.x, v.y); }
	Vec2 sub(Vec2 a, Vec2 b){ Vec2 x = copy(a); x.sub(b); return x; }
	Vec2 add(Vec2 a, Vec2 b){ Vec2 x = copy(a); x.add(b); return x; }
	double heading(){ return Vec2D.polar(bot.getSteerHeading(timestamp),1).h();}
	double turret(){ return Vec2D.polar(bot.getTurretHeading(timestamp),1).h();}
	
	boolean inCases(int n, int N){
		return Math.random()<(n/N);
	}
	boolean inCases(float n){
		double rnd = Math.random();
		//System.out.println("rnd = "+rnd+", n="+n);
		return rnd<n;
	}
	
	class Timer{
		long time;
		long lastTime;
		public Timer(long time){ this.time=time; lastTime=timestamp; }
		boolean isTime(){ 
			boolean res = (timestamp - lastTime)>=time;
			if(res){ lastTime = timestamp; return true; }
			return false;
		}
	}
	
	//------------------------------------------------------------
	
	Vec2 goal = null;
	Vec2 obstacle = null;
	
	class CheckGripper extends BT.Task{
		public BT.STATUS step(){
			if(bot.getObjectInGripper(timestamp)>-1){
				bot.setGripperFingers(timestamp, 0);
				return BT.STATUS.True;
			}
			return BT.STATUS.Progress;
		}

	}
	class SearchObstacle extends BT.Task{
		public BT.STATUS step(){
			Vec2D pos = myPos();
			Vec2D closest = closestObstacle();
			obstacle = null;
			if(closest!=null && closest.sub(pos).r()<2){
				obstacle = closest.Vec2(); 
				//System.out.println("obst="+obstacle);
			}
			return BT.STATUS.Progress;
		}
	}
	
	class SetForwardGoal extends BT.Task{
		Vec2 ggoal = null;
		public BT.STATUS step(){
			if(ggoal==null){
				ggoal = myPos().add(Vec2D.polar(heading(),10)).Vec2();//.rotated(Math.PI).Vec2();
				System.out.println(ggoal);
			}
			goal = copy(ggoal); //myPos().add (  Vec2D.fromVec2(ggoal).sub(myPos()).changedR(1)   ).Vec2();
			//System.out.println("cgoal="+goal.toString() + ", mypos="+myPos().Vec2());
			return BT.STATUS.Progress;
		}
	}
	class SetWanderingGoal extends BT.Task{
		double dir = 1;
		final double rotate_step = 10* Math.PI / 180f;
		Timer timer = new Timer(1000);
		public BT.STATUS step(){
			Vec2D mvec = Vec2D.polar(heading(), 10);
			if(goal!=null){
				mvec = new Vec2D(goal).sub(myPos());
			}
			if(timer.isTime() && inCases(1/10f) || goal==null){
				//System.out.println("change direction : "+dir);
				dir = (inCases(2/10f)?-dir:dir);
				mvec = mvec.rotated( rotate_step * dir );
				goal = myPos().add (  mvec   ).Vec2();
			}
			return BT.STATUS.Progress;
		}
	}
	
	
	class MoveToGoal extends BT.Task{
		
		BT.STATUS onNoGoal(){
			bot.setSpeed(timestamp, 0);
			return BT.STATUS.Progress;
		}
		
		double turnAngleToGoal(){
			return 
			Vec2D.heading(
					// heading from current position to goal without current heading
					new Vec2D(goal).sub(myPos()).h() - heading()
			);
		}
		double correntTurnAngleForAvoidObstacle(double tg){
			//calculate vector from current position to goal (lenght=1)
			Vec2D totar = Vec2D.polar(tg, 2);
			//vector from current position to obstacle rotate 180 deg (opposite vector)
			Vec2D avoid = Vec2D.fromVec2(obstacle).sub(myPos()).rotated(Math.PI); 
			//change vector length by formula of potential field : 1/(2R^2)
			avoid.r(1/Math.pow(avoid.r()*2,2));
			return totar.add(avoid).h();
		}
		
		public BT.STATUS step(){
			double speed = 0.5;
			if(goal==null){
				return onNoGoal();
			}
			
			double tg = turnAngleToGoal();
			//System.out.println("heading = "+heading());
			//System.out.println("tg="+tg);
			
			if(obstacle!=null){
				Vec2D toObst = new Vec2D(obstacle).sub(myPos()).rotated(-heading());
				//System.out.println("OBST! = "+toObst.r()+","+toObst.h());
				if(toObst.r()<bot.RADIUS*2 && Math.abs(toObst.h())<Units.DegToRad(30)){
					//System.out.println("ESTOP");
					speed = 0f;
				}
				tg = correntTurnAngleForAvoidObstacle(tg);
			}
			
			final double turnStep = 0.1;
			if(Math.abs(tg)>turnStep){
				if(tg<0) tg = -turnStep;
				if(tg>0) tg = +turnStep;
			}
			
			System.out.println("tg="+tg);
			bot_turn(tg);
			
			bot.setSpeed(timestamp, speed);
			
			return BT.STATUS.Progress;
		}

	}
	
	class Wander extends BT.Task{
		long turntime = 0;		
		public BT.STATUS step(){
			bot.setSpeed(timestamp, 0.5);
			if(Math.random() > 0.7 && (timestamp-turntime)>5000){
				bot_turn(timestamp, Units.DegToRad(45*(Math.random()>0.5?-1:1)));
				turntime = timestamp;
			}
			return BT.STATUS.Progress;
		}

	}
	

	
	class Stop extends BT.Task{
		public BT.STATUS step(){
			bot.setSpeed(timestamp, 0);
			return BT.STATUS.True;
		}

	}
	
	BT.Task behavior = null;
	
	public void configure()
	{
		bot = abstract_robot;
		bot.setObstacleMaxRange(5.0); 
		bot.setBaseSpeed(0.4*abstract_robot.MAX_TRANSLATION);
		bot.setGripperHeight(-1,0);   
		bot.setGripperFingers(-1,1);  
	
		behavior = new BT.Seq()
		.add(
			new BT.Par()
			//.add(new CheckGripper())
			.add(new SearchObstacle())
			.add(new SetForwardGoal())
			//.add(new SetWanderingGoal())
			.add(new MoveToGoal())
			//.add(new SetForwardGoal())
			//.add(new Wander())
		)
		.add(new Stop());
		behavior.reset();
	}

	
	public int takeStep()
	{
		if(bot.getID()!=1) return 0;
		long	t = abstract_robot.getTime();
		timestamp = t;
		
		behavior.timestamp = t;
		behavior.step();
		
		return(CSSTAT_OK);
		}
	}
